

import java.io.BufferedInputStream;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.util.logging.Level;
import java.util.logging.Logger;
import lejos.pc.comm.*;

/**
 * Provides  Bluetooth communications services to RCNavitationControl:<br>
 * 1. connect to NXT
 * 2. send commands  using the Command emum
 * 3. receives robot position
 * @author Roger
 */
public class RobotCommunicator
{

	/**
	 * constructor establishes  call back path of the RCNavigationControl
	 * @param rcLigthSensor
	 */
	public RobotCommunicator(MainGUi rcLigthSensor)
	{
		this.control = rcLigthSensor;
		System.out.println(" RCNavComms start");
	}

	/**
	 * connects to NXT using Bluetooth
	 * @param name of NXT
	 * @param address  bluetooth address
	 */
	public boolean connect(String name, String address)
	{
		System.out.println(" connecting to " + name + " " + address);
		connector = new NXTConnector();
		boolean connected = connector.connectTo(name, address, NXTCommFactory.BLUETOOTH);
		System.out.println(" connect result " + connected);
		if (!connected)
		{
			return connected;
		}

		dataIn = connector.getDataIn();
		dataOut = connector.getDataOut();
		if (dataIn == null)
		{
			connected = false;
			return connected;
		}
		Thread reader = new Thread(new Reader());
		reader.start();
		return connected;
	}

	/**
	 * inner class to monitor for an incoming message after a command has been sent <br>
	 * calls showRobotPosition() on the controller
	 */
	class Reader extends Thread
	{

		public boolean reading = true;
		int count = 0;
		long runtime = 0;
		int trackCompleted = 0;
		boolean isRunning = false;
		private int displaycounter;
		private boolean executeCompleted = false;
		public void run()
		{
			isRunning = true;
			try {
				while (isRunning)
				{
					if (reading)  //reads one message at a time
					{
						//						System.out.println("reading ");
						int x = 0;
						int y = 0;
						int h = 0;
						float distance = 0;
						boolean ok = true;
						int code = dataIn.readInt();
						if(code==-1) {
							y=dataIn.readInt();
							control.newBarcodeDetected(y);
							System.out.println("Barcode aangekomen : " + y);
							//							int detectedcode = dataIn.readInt();
							//							control.codeDetected(detectedcode);
							//							System.out.println("codeDetected aangekomen : " + detectedcode);
							if(executeCompleted) {
								control.fase2(trackCompleted);
								executeCompleted = false;
							}
						}
						else if(code == -2){
							float d = dataIn.readFloat();
							int state = dataIn.readInt();
							System.out.println("state : " + state);
							float speed = dataIn.readFloat();
							System.out.println("speed : " + speed);
							control.visualizeData(d, state, d);
						}
						else if(code == -3) {
							trackCompleted = dataIn.readInt();
							executeCompleted  = true;
							System.out.println("track Completed!");
						}
						else {
							x = dataIn.readInt();
							y = dataIn.readInt();
							h = dataIn.readInt();
							System.out.println("code : " + code + " data : " + x + " " + y + " " + h);
							control.visualizeData(x, y, h);}
						ok = true;
						if (ok && displaycounter==1)
						{
							reading = false;
							displaycounter =0;
						}
						try
						{
							Thread.sleep(1);
						} catch (InterruptedException ex)
						{
							Logger.getLogger(RobotCommunicator.class.getName()).log(Level.SEVERE, null, ex);
						}
						reading = true;
						displaycounter++;
					}
				}// if reading
				Thread.yield();
			}//while is running
			catch (Exception e) {
				e.printStackTrace();
			}
		}
	}
	/**
	 * sends a command with a variable number of float parameters.
	 *  see http://java.sun.com/docs/books/tutorial/java/javaOO/arguments.html
	 * the section on Arbitrary Number of Arguments
	 * @param c a Command enum
	 * @param data  an array of floats built from the collection list parameters.
	 */
	//  public void send(Command c, float... data)
	//  {
	//    while(reader.reading)
	//    {
	//      Thread.yield();
	//    }
	//    try
	//    {
	//      dataOut.writeInt(c.ordinal());  // convert the enum to an integer
	//      for (float d : data)  // iterate over the   data   array
	//      {
	//        dataOut.writeFloat(d);
	//      }
	//      dataOut.flush();
	//    } catch (IOException e)
	//    {
	//      System.out.println(" send throws exception  " + e);
	//    }
	//    reader.reading = true;  //reader: listen for response
	//  }
	//	public static int toInt( byte[] bytes ) {
	//		int result = 0;
	//		for (int i=0; i<4; i++) {
	//			result = ( result << 8 ) - Byte.MIN_VALUE + (int) bytes[3-i];
	//		}
	//		return result;
	//	}
	/**
	 * used by reader
	 */
	private DataInputStream dataIn;
	/**
	 * used by send()
	 */
	private DataOutputStream dataOut;
	private NXTConnector connector;
	private MainGUi control;
}
